package hdgeom.primitives;

import java.awt.geom.Line2D;

import processing.core.PVector;
import simpleGeom.Vec2D;

public class Vec3D extends PVector {

	public Vec3D(float x, float y, float z) {
		this.x = x;
		this.y = y;
		this.z = z;
	}

	public Vec3D(double x, double y, double z) {
		this.x = (float) x;
		this.y = (float) y;
		this.z = (float) z;
	}

	public Vec3D(float x, float y) {
		this.x = x;
		this.y = y;
	}

	public Vec3D() {
	}

	public boolean equalPosition3D(Vec3D v, float dist) {
		if (this.dist(v) < dist)
			return true;
		return false;
	}

	public boolean equalPosition(Vec3D v) {
		if (v.x == x && v.y == y && v.z == z)
			return true;
		return false;
	}
	public boolean equalPosition(PVector v) {
		if (v.x == x && v.y == y && v.z == z)
			return true;
		return false;
	}

	public void setPosition(float x, float y, float z) {
		this.x = x;
		this.y = y;
		this.z = z;
	}

	public Vec3D getCopy() {
		return new Vec3D(x, y, z);
	}

	public void add(Vec3D v) {
		this.x += v.getX();
		this.y += v.getY();
		this.z += v.getZ();
	}

	public void sub(Vec3D v) {
		this.x -= v.getX();
		this.y -= v.getY();
		this.z -= v.getZ();
	}

	public float mag() {
		return (float) Math.sqrt(magSqr());
	}

	public float magSqr() {
		return x * x + y * y + z * z;
	}

	public void mult(float f) {
		this.x *= f;
		this.y *= f;
		this.z *= f;
	}

	public void div(float f) {
		this.x /= f;
		this.y /= f;
		this.z /= f;
	}

	public float distSqr(float x, float y, float z) {
		float dX = this.x - x;
		float dY = this.y - y;
		float dZ = this.z - z;
		return dX * dX + dY * dY + dZ * dZ;
	}

	public float distSqr(Vec3D v) {
		return distSqr(v.getX(), v.getY(), v.getZ());
	}

	public float dist(float x, float y, float z) {
		return (float) Math.sqrt(distSqr(x, y, z));
	}

	public float dist(Vec3D v) {
		return (float) Math.sqrt(distSqr(v));
	}

	public float dot(Vec3D b) {
		return x * b.getX() + y * b.getY() + z * b.getZ();
	}

	public float dot(float cX, float cY, float cZ) {
		return x * cX + y * cY + z * cZ;
	}

	public Vec3D cross(Vec3D b) {
		float cX = y * b.getZ() - z * b.getY();
		float cY = z * b.getX() - x * b.getZ();
		float cZ = x * b.getY() - y * b.getX();
		return new Vec3D(cX, cY, cZ);
	}

	public void normalize() {
		div(mag());
	}

	public void invert() {
		mult(-1);
	}

	public void setLength(float l) {
		// TODO Auto-generated method stub
		this.mult(l / mag());
	}

	@Override
	public String toString() {
		// TODO Auto-generated method stub
		return super.toString() + " x: " + x + " y: " + y + " z: " + z;
	}
	public static float getAngleBetween2D(PVector a, PVector b) {
		
		double angle = Math.atan2(b.y, b.x) - Math.atan2(a.y, a.x);
		if (angle<0)angle+=Math.PI*2;
		if (angle>Math.PI)angle=Math.PI*2-angle;
		return (float)angle;
	}
	public static float getAngle(PVector n1, PVector n2) {
		return (float) Math.atan2(n1.cross(n2).mag(), PVector.dot(n1, n2));
	}

	public float getAngleY() {
		float l2D = (float) Math.sqrt(x * x + y * y);
		return (float) Math.atan2(l2D, z);
	}

	public float getAngleZZ() {
		return (float) Math.atan2(y, x);
	}

	float getYAngle(float x1, float y1, float z1, float x2, float y2, float z2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		float dZ = z2 - z1;
		float dXY = (float) Math.sqrt(dX * dX + dY * dY);
		return (float) Math.atan2(dZ, -dXY);
	}

	float getZAngle(float x1, float y1, float x2, float y2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		return (float) Math.atan2(dY, dX);
	}

	public float getZ() {
		return z;
	}

	public Vec3D getOrthoVector() {
		// one of endless;
		Vec3D v = null;
		if (z != 0)
			v = new Vec3D(1, 1, -(x + y) / z);
		else if (y != 0)
			v = new Vec3D(1, -(x + z) / y, 1);
		else if (x != 0)
			v = new Vec3D(-(y + z) / x, 1, 1);
		v.normalize();
		return v;
	}

	public void setZ(float z) {
		this.z = z;
	}

	public static Vec3D cross(Vec3D a, Vec3D b) {
		float x = a.getY() * b.getZ() - a.getZ() * b.getY();
		float y = a.getZ() * b.getX() - a.getX() * b.getZ();
		float z = a.getX() * b.getY() - a.getY() * b.getX();
		return new Vec3D(x, y, z);
	}

	public static float dot(Vec3D a, Vec3D b) {
		return a.getX() * b.getX() + a.getY() * b.getY() + a.getZ() * b.getZ();
	}

	public static float mag(Vec3D v) {
		return (float) Math.sqrt(magSqr(v));
	}

	public static float magSqr(Vec3D v) {
		return v.getX() * v.getX() + v.getY() * v.getY() + v.getZ() * v.getZ();
	}

	public static Vec3D div(Vec3D p, float f) {
		return new Vec3D(p.getX() / f, p.getY() / f, p.getZ() / f);
	}

	public static float distSqr(Vec3D v1, Vec3D v2) {
		float dX = v1.getX() - v2.getX();
		float dY = v1.getY() - v2.getY();
		float dZ = v1.getZ() - v2.getZ();

		return dX * dX + dY * dY + dZ * dZ;
	}

	public static float dist(float x1, float y1, float z1, float x2, float y2,
			float z2) {
		return (float) Math.sqrt(distSqr(x1, y1, z1, x2, y2, z2));
	}

	public static float distSqr(float x1, float y1, float z1, float x2,
			float y2, float z2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		float dZ = z2 - z1;

		return dX * dX + dY * dY + dZ * dZ;
	}

	public static float dist(Vec3D v1, Vec3D v2) {
		float d = distSqr(v1, v2);
		return (float) Math.sqrt(d);
	}

	public static Vec3D normalize(Vec3D p) {
		return div(p, mag(p));
	}

	public static Vec3D getNormal(Vec3D a, Vec3D b, Vec3D c) {
		Vec3D ab = Vec3D.sub(b, a);
		Vec3D ac = Vec3D.sub(c, a);
		Vec3D cross = Vec3D.cross(ac, ab);

		cross.normalize();
		if (Float.isNaN(cross.x)) {
			System.out.println("b==a:" + (b == a));
			System.out.println("b==c:" + (b == c));
			System.out.println("a==c:" + (a == c));
			System.out.println("ab: " + ab);
			System.out.println("ac: " + ac);
			System.out.println("b: " + b);
			System.out.println("c: " + c);
			System.out.println(cross);
		}
		return cross;
	}

	public static Vec3D rotateAroundLine(Vec3D p, Vec3D o, Vec3D vec,
			float angle) {
		// http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/
		// If we multiply this times ⟨x,y,z⟩ we can obtain a function of of
		// ten variables that yields the result of
		// rotating the point (x,y,z) about the line through (a,b,c) with
		// direction vector ⟨u,v,w⟩ (where u2 + v2 + w2 = 1) by the angle
		// θ.
		float x = p.getX();
		float y = p.getY();
		float z = p.getZ();
		float a = o.getX();
		float b = o.getY();
		float c = o.getZ();
		float u = vec.getX();
		float v = vec.getY();
		float w = vec.getZ();
		float cos = (float) Math.cos(angle);
		float sin = (float) Math.sin(angle);
		float newX = (a * (v * v + w * w) - u
				* (b * v + c * w - u * x - v * y - w * z))
				* (1 - cos) + x * cos + (-c * v + b * w - w * y + v * z) * sin;
		float newY = (b * (u * u + w * w) - v
				* (a * u - c * w - u * x - v * y - w * z))
				* (1 - cos) + y * cos + (c * u - a * w + w * x - u * z) * sin;
		float newZ = (c * (u * u + v * v) - w
				* (a * u + b * v - u * x - v * y - w * z))
				* (1 - cos) + z * cos + (-b * u + a * v - v * x + u * y) * sin;
		return new Vec3D(newX, newY, newZ);
	}

	public static Vec3D mult(Vec3D p1, float m) {

		return new Vec3D(p1.getX() * m, p1.getY() * m, p1.getZ() * m);
	}

	public static Vec3D add(Vec3D v1, Vec3D v2) {

		return new Vec3D(v1.getX() + v2.getX(), v1.getY() + v2.getY(),
				v1.getZ() + v2.getZ());
	}

	public static Vec3D sub(Vec3D v1, Vec3D v2) {

		return new Vec3D(v1.getX() - v2.getX(), v1.getY() - v2.getY(),
				v1.getZ() - v2.getZ());
	}

	public static Vec3D getBetween(Vec3D v1, Vec3D v2, float f) {
		float cX = f * (v2.getX() - v1.getX()) + v1.getX();
		float cY = f * (v2.getY() - v1.getY()) + v1.getY();
		float cZ = f * (v2.getZ() - v1.getZ()) + v1.getZ();
		return new Vec3D(cX, cY, cZ);
	}

	public float getX() {
		return x;
	}

	public void setX(float x) {
		this.x = x;
	}

	public float getY() {
		return y;
	}

	public void setY(float y) {
		this.y = y;
	}

	final static float SMALL_NUM = 0.0000001f;

	public static Vec3D getNormalUnscaled(Vec3D p1, Vec3D p2, Vec3D p3) {
		Vec3D v1 = Vec3D.sub(p2, p1);
		Vec3D v2 = Vec3D.sub(p3, p1);
		Vec3D norm = v1.cross(v2);
		return norm;
	}

	public static float getTriangleArea(Vec3D a, Vec3D b, Vec3D c) {
		return getTriangleArea(a.getX(), a.getY(), a.getZ(), b.getX(),
				b.getY(), b.getZ(), c.getX(), c.getY(), c.getZ());
	}

	public static float getTriangleArea(float xa, float ya, float za, float xb,
			float yb, float zb, float xc, float yc, float zc) {
		return (float) (0.5f * Math.sqrt(Math.pow(
				determinant(xa, xb, xc, ya, yb, yc, 1, 1, 1), 2)
				+ Math.pow(determinant(ya, yb, yc, za, zb, zc, 1, 1, 1), 2)
				+ Math.pow(determinant(za, zb, zc, xa, xb, xc, 1, 1, 1), 2)));
	}

	// http://stackoverflow.com/questions/1406029/how-to-calculate-the-volume-of-a-3d-mesh-object-the-surface-of-which-is-made-up
	public static float signedVolumeOfTriangle(Vec3D p1, Vec3D p2, Vec3D p3) {
		float v321 = p3.getX() * p2.getY() * p1.getZ();
		float v231 = p2.getX() * p3.getY() * p1.getZ();
		float v312 = p3.getX() * p1.getY() * p2.getZ();
		float v132 = p1.getX() * p3.getY() * p2.getZ();
		float v213 = p2.getX() * p1.getY() * p3.getZ();
		float v123 = p1.getX() * p2.getY() * p3.getZ();
		return (1.0f / 6.0f) * (-v321 + v231 + v312 - v132 - v213 + v123);
	}

	public static float determinant(float a, float b, float c, float d,
			float e, float f, float g, float h, float i) {
		return (a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c
				* e * g);
	}

	public static Vec3D getGravityCenter(Vec3D[] points) {
		Vec3D p = new Vec3D();
		for (int i = 0; i < points.length; i++) {
			Vec3D cP = points[i];
			p.add(cP);
		}
		p.mult(1f / points.length);
		return p;
	}

	public static float getArea2D(Vec3D[] points) {
		float area = 0;
		int j;
		for (int i = 0; i < points.length; i++) {
			j = i + 1;
			if (j == points.length)
				j = 0;
			Vec3D p1 = points[i];
			Vec3D p2 = points[j];
			area += (p1.getX() + p2.getX()) * (p2.getY() - p1.getY());
		}
		return area / 2f;
	}

	public static float getPerimeter(Vec3D[] points) {
		float sum = 0;
		for (int i = 0; i < points.length; i++) {
			Vec3D n1 = points[i];
			Vec3D n2 = points[(i + 1) % points.length];
			sum += Vec3D.dist(n1, n2);
		}
		return sum;
	}

	public static Vec3D intersect_VerRayTriangle(Vec3D p0, Vec3D p1, Vec3D p2,
			Vec3D r0, Vec3D r1) {

		// triangle vectors

		// is I inside T?

		// sameNormal
		if (Line2D.relativeCCW(p0.getX(), p0.getY(), p1.getX(), p1.getY(),
				p2.getX(), p2.getY()) == 1) {
			Vec3D t = p0;
			p0 = p1;
			p1 = t;
		}

		Vec3D i = rayTriIntersection(p0, p1, p2, r0, r1);
		if (i == null) {
			return null;
		}
		if (Line2D.relativeCCW(p2.getX(), p2.getY(), p0.getX(), p0.getY(),
				r0.getX(), r0.getY()) == 1)
			return null;
		if (Line2D.relativeCCW(p0.getX(), p0.getY(), p1.getX(), p1.getY(),
				r0.getX(), r0.getY()) == 1)
			return null;
		if (Line2D.relativeCCW(p1.getX(), p1.getY(), p2.getX(), p2.getY(),
				r0.getX(), r0.getY()) == 1)
			return null;

		return new Vec3D();
		/*
		 * Plane plane=new Plane(p2,p1,p0); float d=plane.getDistanceSigned(r0);
		 * if (d<0)return new Vec3D(); else return null;
		 */
		// return i; // I is in T
	}

	public static Vec3D rayTriIntersection(Vec3D p0, Vec3D p1, Vec3D p2,
			Vec3D r0, Vec3D r1) {
		Vec3D e1 = Vec3D.sub(p1, p0);
		Vec3D e2 = Vec3D.sub(p2, p0);
		Vec3D d = Vec3D.sub(r1, r0);
		Vec3D h = Vec3D.cross(d, e2);
		float a = Vec3D.dot(e1, h);
		if (a > -SMALL_NUM && a < SMALL_NUM)
			return null;
		float f = 1f / a;
		Vec3D s = Vec3D.sub(r0, p0);
		float u = f * Vec3D.dot(s, h);
		if (u < 0 || u > 1)
			return null;
		Vec3D q = Vec3D.cross(s, e1);
		float v = f * Vec3D.dot(d, q);
		if (v < 0 || u + v > 1)
			return null;
		float t = f * Vec3D.dot(e2, q);
		if (t > SMALL_NUM) {
			d.mult(t);
			d.add(r0);
			return d;
		}
		return null;
	}

	public static float getDistanceSignedToPlane(Vec3D p0, Vec3D p1, Vec3D p2,
			float x, float y, float z) {
		// this.p=new Vec3D(p);
		Vec3D normal = Vec3D.getNormal(p0, p1, p2);
		normal.normalize();
		float d0 = -normal.dot(p0);
		return normal.dot(x, y, z) + d0;
	}

	public static float getDistanceSignedToPlane(Vec3D p0, Vec3D p1, Vec3D p2,
			Vec3D v) {
		return getDistanceSignedToPlane(p0, p1, p2, v.x, v.y, v.z);
	}
}
